#ifndef SLAM_MAPPING_INTERNAL_2D_LINE_EXTRACTION_H
#define SLAM_MAPPING_INTERNAL_2D_LINE_EXTRACTION_H

#include <glog/logging.h>
#include <Eigen/Dense>
#include <boost/array.hpp>
#include <cmath>
#include <vector>

#include "slam/sensor/point_cloud.h"

namespace slam {
namespace mapping {

struct LinePointInfo {
  double range;
  double angle;
};

struct LineExtractionParams {
  double bearing_var = 1e-6;
  double range_var = 4e-4;
  // Change in angle (rad) threshold to stop iterating least squares
  // (`least_sq_radius_thresh` must also be met)
  double least_sq_angle_thresh = 0.0001;
  // Change in radius (m) threshold to stop iterating least squares
  // (`least_sq_angle_thresh` must also be met).
  double least_sq_radius_thresh = 0.0001;
  double min_line_length = 0.05;
  double max_line_length = 0.25;
  // The maximum distance between two points in the same line (m).
  double max_line_gap = 0.05;
  // When performing "split" step of split and merge, a split between two points
  // results when the two points are at least this far apart (m).
  double min_split_dist = 0.05;
  // Points who are at least this distance from all their neighbours are
  // considered outliers (m).
  double outlier_dist = 0.1;
  int min_line_points = 6;
};

struct LinePointParams {
  std::vector<double> a;
  std::vector<double> ap;
  std::vector<double> app;
  std::vector<double> b;
  std::vector<double> bp;
  std::vector<double> bpp;
  std::vector<double> c;
  std::vector<double> s;
};

struct LineInfo {
  boost::array<double, 2> start;
  boost::array<double, 2> end;
  boost::array<double, 2> center;
  int num_points;
  double length;
  double angle;
  double radius;
};

inline double PIToPI(double angle) {
  angle = fmod(angle, 2 * M_PI);
  if (angle >= M_PI) angle -= 2 * M_PI;
  return angle;
}

class Line {
 public:
  Line(const sensor::PointCloud& laser_data,
       const LineExtractionParams& params);
  ~Line();

  void SetLineParas(const double angle, const double radius,
                    const boost::array<double, 4>& covariance,
                    const boost::array<double, 2>& start,
                    const boost::array<double, 2>& end);
  // Get methods for the line parameters
  double GetAngle() const;
  const boost::array<double, 4>& GetCovariance() const;
  const boost::array<double, 2>& GetEnd() const;
  double GetRadius() const;
  const boost::array<double, 2>& GetStart() const;
  // Methods for line fitting
  double DistToPoint(const sensor::RangefinderPoint& point);
  void EndpointFit();
  void LeastSqFit();
  double Length() const;
  unsigned int NumPoints() const;
  void ProjectEndpoints();
  const sensor::PointCloud& GetLaserData() const;

 private:
  void AngleFromEndpoints();
  void AngleFromLeastSq();
  double AngleIncrement();
  void CalcCovariance();
  void CalcPointCovariances();
  void CalcPointParameters();
  void CalcPointScalarCovariances();
  void RadiusFromEndpoints();
  void RadiusFromLeastSq();

 private:
  sensor::PointCloud laser_data_;
  std::vector<LinePointInfo> points_info_;
  LineExtractionParams params_;
  LinePointParams p_params_;
  // Point variances used for least squares
  std::vector<double> point_scalar_vars_;
  std::vector<boost::array<double, 4> > point_covs_;
  double p_rr_;
  // Line parameters
  double angle_;
  double radius_;
  boost::array<double, 2> start_;
  boost::array<double, 2> end_;
  boost::array<double, 4> covariance_;
};  // class Line

class LineExtraction {
 public:
  LineExtraction();
  LineExtraction(const LineExtractionParams& params);
  ~LineExtraction();

  void ExtractLines(const sensor::PointCloud& laser_data,
                    std::vector<LineInfo>* lines_info);

 private:
  double DistBetweenPoints(const sensor::RangefinderPoint& point_1,
                           const sensor::RangefinderPoint& point_2);
  double ChiSquared(const Eigen::Vector2d& dL, const Eigen::Matrix2d& P_1,
                    const Eigen::Matrix2d& P_2);
  void FilterLaserPoints(const sensor::PointCloud& laser_data);
  void FilterLines();
  void MergeLines();
  void Split(const sensor::PointCloud& laser_data);

 private:
  const LineExtractionParams params_;
  sensor::PointCloud filtered_laser_data_;
  // Line data
  std::vector<Line> lines_;
};

}  // namespace mapping
}  // namespace slam

#endif
